/* SkyTraq Venus634FLPx GPS driver */

/* Based on the following documents:
 * AN0003 Binary Messages of SkyTraq Venus 6 (v1.4.14, dated 17 July 2009)
 * Venus634FLPx Datasheet (undated)
 */

/* Copyright (C) 2010-2011 David Zanetti
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "global.h"
#include <util/delay.h>

#include "serial.h"
#include "clock.h"
#include "venus634.h"

/* some useful defines */
#define START1 0xA0
#define START2 0xA1
#define END1 0x0D
#define END2 0x0A

/* All binary packets are structured such:
 * START1 START2 len[2] type ... cs END1 END2 */

/* INPUT FORMATS */

/* packet types */
/* 0x01 - System Restart */
#define M_RESTART 0x01
typedef struct {
 uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
 uint16_t utc_year; /* >= 1980 */
 uint8_t utc_month;
 uint8_t utc_day;
 uint8_t utc_hour;
 uint8_t utc_minute;
 uint8_t utc_second;
 int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
 int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
 int16_t altitude; /* -1000 to 18300, meters */
} m_restart_t;

/* 0x02 - Query Software Version */
#define M_QUERYVER 0x02
typedef struct {
 uint8_t software_type; /* 01 = System Code */
} m_queryver_t;

/* 0x03 - Query Software CRC */
#define M_QUERYCRC 0x03
typedef struct {
 uint8_t software_type; /* 01 = System Code */
} m_querycrc_t;

/* 0x04 - Set Factory Defaults */
#define M_FACTRESET 0x04
typedef struct { 
 uint8_t type; /* 01 = reboot after factory setting reset */
} m_factreset_t;

/* 0x05 - Serial Port Configuration */
#define M_SERCONF 0x05
typedef struct {
 uint8_t port; /* 00 = COM1 (the only port) */
 uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400; 
                  04 = 57600; 05 = 115200; */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_serconf_t;

/* 0x08 - Configure NMEA intervals */
#define M_NMEACONF 0x08
typedef struct {
 uint8_t gga_int; /* seconds for all of these, 0 = off */
 uint8_t gsa_int;
 uint8_t gsv_int;
 uint8_t gll_int;
 uint8_t rmc_int;
 uint8_t vtg_int;
 uint8_t zda_int; /* is this supported on current roms? */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_nmeaconf_t;

/* 0x09 - Configure comms mode BINARY/NMEA */
#define M_MODECONF 0x09
typedef struct {
 uint8_t mode; /* 00 = none, 01 = NMEA, 02 = binary */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_modeconf_t;

/* 0x0c - Configure power mode */
#define M_POWERCONF 0x0c
typedef struct {
 uint8_t mode; /* 00 = normal; 01 = power save */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH; 02 = temporary */
} m_powerconf_t;

/* 0x0e - Conf position update rate */
#define M_POSHZ 0x0e
typedef struct {
 uint8_t rate; /* 1,2,4,5,8,10 (decimal), rate in Hz */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_poshz_t;

/* 0x10 - Query position rate */
#define M_QUERYPOSHZ 0x10
/* This packet has no payload */

/* 0x11 - Configure BINARY navigation message */
#define M_NAVINT 0x11
typedef struct {
 uint8_t nav_int; /* seconds, 0 = disable */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_navint_t;

/* 0x29 - Configure Datum */
#define M_DATUMCONF 0x29
/* This is a complex type, refer to datasheet for full values */
typedef struct {
 uint16_t index;
 uint8_t ellip_index;
 int16_t delta_x;
 int16_t delta_y;
 int16_t delta_z;
 uint32_t semi_major_axis;
 uint32_t inversed_flattening;
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_datumconf_t;

/* 0x2d - Query Datum */
#define M_QUERYDATUM 0x2d
/* no payload */

/* 0x30 - Get ephemeris used */
#define M_QUERYEPH 0x30
typedef struct {
 uint8_t sv; /* 0 = all, 1-32 is sat number */
} m_queryeph_t;

/* 0x31 - Set ephemeris */
#define M_SETEPH 0x31
typedef struct {
 uint8_t sv; /* sat id */
 uint8_t subframe0[28];
 uint8_t subframe1[28];
 uint8_t subframe2[28];
} m_seteph_t;

/* 0x37 - Configure WAAS */
#define M_WAASCONF 0x37
typedef struct {
 uint8_t enable; /* 0 = disable; 1 = enable */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_waasconf_t;

/* 0x38 - Query WAAS */
#define M_QUERYWAAS 0x38
/* no payload */

/* 0x39 - Configure position pinning */
#define M_PINCONF 0x39
typedef struct {
 uint8_t enable; /* 0 = disable; 1 = enable */
} m_pinconf_t;

/* 0x3a - Query position pinning */
#define M_QUERYPIN 0x3a
/* no payload */

/* 0x3b - Configure positioning pinning parameters */
#define M_PINPARACONF 0x3b
typedef struct {
 uint16_t speed; /* km/h */
 uint16_t count; /* seconds */
 uint16_t un_speed; /* km/h */
 uint16_t un_count; /* seconds */
 uint16_t un_dist; /* meters */
} m_pinparaconf_t;

/* 0x3c - Navigation mode */
#define M_NAVMODECONF 0x3c
typedef struct {
 uint8_t mode; /* 0 = car; 1 = pedestrian */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_navmodeconf_t;

/* 0x3d - query nav mode */
#define M_QUERYNAVMODE 0x3d
/* no payload */

/* 0x3e - 1PPS mode */
#define M_PPSCONF 0x3e
typedef struct {
 uint8_t mode; /* 0 = off, 1 = 3D fix; 2 = single sat */
 uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
} m_ppsconf_t;

/* 0x3f - query 1pps mode */
#define M_QUERYPPS 0x3f
/* no payload */

/* OUTPUT MESSAGE FORMATS */

/* 0x80 - software version */
#define MO_SOFTVER 0x80
typedef struct {
 uint8_t type; /* 1 = system code */
 uint32_t kernel; /* XX maj min patch */
 uint32_t odm; /* XX maj min patch */
 uint32_t rev; /* XX yy mm dd */
} mo_softver_t;

/* 0x81 - software crc */
#define MO_SOFTCRC 0x81
typedef struct {
 uint8_t type; /* 1 = system code */
 uint16_t crc; /* crc value */
} mo_softcrc_t;

/* 0x83 - ACK */
#define MO_ACK 0x83
typedef struct {
 uint8_t message_type; /* the message being ACKd */
} mo_ack_t;

/* 0x84 - NACK */
#define MO_NACK 0x84
typedef struct {
 uint8_t message_type; /* the message being NACKd */
} mo_nack_t;

/* 0x86 - position update */
#define MO_POSHZ 0x86
typedef struct {
 uint8_t rate; /* see M_POSHZ */
} mo_poshz_t;

/* 0xa8 - navigation data */
#define MO_NAVDATA 0xa8
typedef struct {
 uint8_t fixmode; /* 0 = none; 1 = 2D, 2 = 3D; 3 = 3D+DGPS */
 uint8_t svnum; /* number of sats in fix */
 uint16_t gps_week; /* GPS Week Number */
 uint32_t gps_tow; /* Time of week, seconds, 1/100th resolution */
 int32_t lattitde; /* 1/1e-7 (???) degree, positive north */
 int32_t longitude; /* 1/1e-7 (???) degree, positive east */
 uint32_t elips_alt; /* meters, 1/100th */
 uint32_t sea_alt; /* meters, 1/100th */
 uint16_t gdop; /* 1/100th */
 uint16_t pdop; /* 1/100th */
 uint16_t hdop; /* 1/100th */
 uint16_t vdop; /* 1/100th */
 uint16_t tdop; /* 1/100th */
 int32_t ecef_x; /* meters, 1/100th */
 int32_t ecef_y; /* meters, 1/100th */
 int32_t ecef_z; /* meters, 1/100th */
 int32_t ecef_vx; /* meters, 1/100th */
 int32_t ecef_vy; /* meters, 1/100th */
 int32_t ecef_vz; /* meters, 1/100th */
} mo_navdata_t;

/* 0xae - datum */
#define MO_DATUM 0xae
typedef struct {
 uint16_t index; /* ... umm */
} mo_datum_t;

/* 0xb1 - ephemeris data */
#define MO_EPHEMERIS 0xb1
typedef struct {
 uint16_t sv; /* sat ID */
 uint8_t subframe0[28];
 uint8_t subframe1[28];
 uint8_t subframe2[28];
} mo_eph_t; 

/* 0xb3 - waas status */
#define MO_WAAS 0xb3
typedef struct {
 uint8_t status; /* 0 = disable; 1 = enable */
} mo_waas_t;

/* 0xb4 - pin status */
#define MO_PIN 0xb3
typedef struct {
 uint8_t status; /* 0 = disable; 1 = enable */
} mo_pin_t;

/* 0xb5 - navigation mode status */
#define MO_NAVMODE 0xb5
typedef struct {
 uint8_t status; /* 0 = disable; 1 = enable */
} mo_navmode_t;

/* 0xb6 - 1pps mode */
#define MO_PPS 0xb6
typedef struct {
 uint8_t status; /* 0 = disable; 1 = enable */
} mo_pps_t;

/* END BINARY MESSAGES */

#define MAX_PKTLEN 64

/* send a binary message, any processing is done async by
   main processing loop */
void venus634_send(uint8_t type, void *payload, uint16_t len);

/* compute CRC for the given type and payload */
uint8_t venus634_crc(uint8_t type, void *payload, uint16_t len);

/* initalise the appropriate mode on the GPS */

void venus634_init(void) {
 /* the restart message */
 m_restart_t restart;

 serial1_init(9600);

 /* since we pre-configured the default to be binary, and nav
    messages are disabled, we only need to check we can talk
    to the GPS okay */

 /* force a cold reset since this is an init */
 /* when the ACK to this is picked up by the main loop, we know
    the GPS is talking to us */
 memset(&restart,0,sizeof(m_restart_t));
 restart.start_mode = 0x03; /* COLD */
 
 venus634_send(M_RESTART,&restart,sizeof(m_restart_t));

}

uint8_t venus634_crc(uint8_t type, void *payload, uint16_t len) {
 uint8_t crc = 0, n;
  
 /* message type first */
 crc ^= type;

 /* and then the payload */
 for (n = 0; n < len; n++) {
  crc ^= *((uint8_t *)payload);
  payload++;
 }
 return crc;
}

void venus634_send(uint8_t type, void *payload, uint16_t len) {
 uint8_t crc, n, rc = 0xff, intype;
 uint16_t inlen;

 /* first compute the CRC of the given message */
 crc = venus634_crc(type, payload, len);

 /* now send the message */
 /* header */
 serial1_tx_cout(START1);
 serial1_tx_cout(START2);
 serial1_tx_cout(len & 0xff);
 serial1_tx_cout(len >> 8);
 /* type and payload */
 serial1_tx_cout(type);
 for (n = 0; n < len; n++) {
  serial1_tx_cout(*(uint8_t *)payload);
  payload++;
 }
 /* checksum and tail */
 serial1_tx_cout(crc);
 serial1_tx_cout(END1);
 serial1_tx_cout(END2);

 /* done! */
 return;
}

/* proccessing states */
#define S_RESET 0
#define S_POSTRESET 1
#define S_START1 2
#define S_START2 3
#define S_LENGTH1 4
#define S_LENGTH2 5
#define S_DISCARD 6
#define S_COPY 7
#define S_ENDCOPY 8
#define S_END 9
#define S_END1 10

void venus634_process(void) {
 uint8_t c, havepkt = 0;
 static uint8_t state = S_RESET, lncnt = 0, type;
 static uint16_t len, n;
 static uint8_t pkt[MAX_PKTLEN+1];
 /* we got a flag for RX, so process the next byte */

 while (flag_serial & FLAG_SERIAL_U1_RX) {

serial0_tx_PGM(PSTR("loop\r\n"));

  c = serial1_rx(); 

  switch (state) {
	case S_RESET:
		/* cold reset state, look for the end of three lines */
		if (c == 0x0a) {
		 lncnt++;
		 if (lncnt == 3) {
	          state = S_POSTRESET; /* we have gotten past the inital reset burst */
		 }
		}
		break;
	case S_POSTRESET:	
		/* we are now into normal packet processing */
		if (c == START1) {
		 state = S_START1; /* start of packet */
		}
		/* otherwise, we just discard */
		break;
	case S_START1:
		/* second marker of packet */
		if (c == START2) {
		 state = S_START2; /* here comes the length */
		} else {
		 state = S_POSTRESET; /* discard mode again */
		}
		break;
	case S_START2:	
		/* the first low-byte part of the length */
	        len = c;
		state = S_LENGTH1;
		break;
	case S_LENGTH1:
		/* second byte is upper part */
		len |= (c << 8);
		state = S_LENGTH2;
	case S_LENGTH2:
		/* this is the type */
		type = c;
		len--;
		if (type != MO_NAVDATA) {
		 /* increase length to cover checksum and tail */
		 len += 3;
		 state = S_DISCARD;
		} else {
		 memset(&pkt,0,MAX_PKTLEN);
		 state = S_COPY;
		}
		break;
	case S_DISCARD:
		/* keep discarding bytes while we have length to read */
		len--;
		if (!len) {
		 state = S_POSTRESET;
		}
		break;
	case S_COPY:
		len--;
		pkt[n] = c;
		n++;
		if (n > MAX_PKTLEN) {
		 state = S_DISCARD;
		 serial0_tx_PGM(PSTR("venus634: overrun\r\n"));
		 break;
		}
		if (!len) {
		 state = S_ENDCOPY;
		}
		break;
  	case S_ENDCOPY:
		/* collect checksum byte, and compute checksum */
		if (venus634_crc(type, &pkt, n) != c) {
		 serial0_tx_PGM(PSTR("venus634: bad checksum\r\n"));
		 state = S_END;
		} else {
#ifdef DEBUG_VENUS634
		 serial0_tx_PGM(PSTR("venus634: rx "));
		 serial0_tx_hex(type);
		 serial0_tx_PGM(PSTR(" msg (good)\r\n"));
#endif
		 havepkt = 1;
		 state = S_END;
		}
		break;
	case S_END:
		/* just discard */
		state = S_END1;
		break;
	case S_END1:
		/* discard tail, and go back to look for new packet */
		state = S_POSTRESET;
		break;
  }

  /* since we'eve processed a character, it might have been the one which
    completed a packet, check if that's the case */

  if (havepkt) {
   switch (type) {
	case MO_NAVDATA:
		serial0_tx_PGM(PSTR("venus634: nav packet "));
		serial0_tx_dec(((mo_navdata_t *)&pkt)->gps_week);
		serial0_tx_PGM(PSTR(" week "));
		serial0_tx_dec(((mo_navdata_t *)&pkt)->gps_tow);
		serial0_tx_PGM(PSTR(" tow\r\n"));
		break;
	case MO_SOFTVER:
		{ 
                  mo_softver_t *data;
 		  data = (mo_softver_t *)&pkt;
		  serial0_tx_PGM(PSTR("venus634: kernel "));
		  serial0_tx_dec((data->kernel) >> 16 & 0xff);
		  serial0_tx_cout('.');
		  serial0_tx_dec((data->kernel) >> 8 & 0xff);
		  serial0_tx_cout('.');
		  serial0_tx_dec((data->kernel) & 0xff);
		  serial0_tx_PGM(PSTR("\r\n"));
		  break;
		 }
   }
  }
 
  /* all done! */

 } /* while we have characters to read */
}
	
void venus634_version(void) {
 m_queryver_t query;

 memset(&query,0,sizeof(m_queryver_t));
 query.software_type = 0x01; /* System */

 venus634_send(M_QUERYVER,&query,sizeof(m_queryver_t));

 return;
}


				
		


 

